#include "Communication/SubMagneticSensor.h"
#include "modules/CtrlModule/ActivePlanner/ObsFollowPlanner.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller
{
    MagneticData DataSet::magnetic_data;
    namespace SubMagneticSensor
    {
         
        static rclcpp::Subscription< sensor_msgs::msg::Range>::SharedPtr  magneticSub;

        void sub_callback(const sensor_msgs::msg::Range::SharedPtr msg)
        {
            //DataSet::magnetic_data.SetDis(msg->range);
            //obs_follow_planner.UpdateObsDistance(msg->range);
            //DataSet::magnetic_data.SetTime();
        }
        void SubMagneticSensorInfo(rclcpp::Node::SharedPtr &node_handle)
        {
            magneticSub = node_handle->create_subscription<sensor_msgs::msg::Range>(
                "magnetic_sensor", rclcpp::SensorDataQoS(), sub_callback);
            std::cout << "SubMagneticSensor SubMagneticSensorInfo" << std::endl;
        }
    } // namespace SubMagneticSensor

} // namespace behavior_controller